﻿//-----------------------------------------------------------------------
// <copyright file="Skeleton3DDataExtract.cs" company="Rhemyst and Rymix">
//     Open Source. Do with this as you will. Include this statement or 
//     don't - whatever you like.
//
//     No warranty or support given. No guarantees this will work or meet
//     your needs. Some elements of this project have been tailored to
//     the authors' needs and therefore don't necessarily follow best
//     practice. Subsequent releases of this project will (probably) not
//     be compatible with different versions, so whatever you do, don't
//     overwrite your implementation with any new releases of this
//     project!
//
//     Enjoy working with Kinect!
// </copyright>
//-----------------------------------------------------------------------

namespace DTWGestureRecognition
{
    using System;
    using System.Windows;
    using System.Windows.Media.Media3D;
    using Microsoft.Research.Kinect.Nui;

    /// <summary>
    /// This class is used to transform the data of the skeleton
    /// </summary>
    internal class Skeleton3DDataExtract
    {
        /// <summary>
        /// Skeleton3DdataCoordEventHandler delegate
        /// </summary>
        /// <param name="sender">The sender object</param>
        /// <param name="a">Skeleton 3Ddata Coord Event Args</param>
        public delegate void Skeleton3DdataCoordEventHandler(object sender, Skeleton3DdataCoordEventArgs a);

        /// <summary>
        /// The Skeleton 3Ddata Coord Ready event
        /// </summary>
        public static event Skeleton3DdataCoordEventHandler Skeleton3DdataCoordReady;

        public static bool isEmptyP1;

        public static bool isEmptyP2;

        /// <summary>
        /// Crunches Kinect SDK's Skeleton Data and spits out a format more useful for DTW
        /// </summary>
        /// <param name="data">Kinect SDK's Skeleton Data</param>
        public static void ProcessData(int Dimensions, SkeletonData dataP1, SkeletonData dataP2)
        {
            Point3D[,] p = new Point3D[2, Dimensions];
            isEmptyP1 = true;
            isEmptyP2 = true;

            // Extract the coordinates of the points.
            if (dataP1 != null)
            {
                isEmptyP1 = false;

                foreach (Joint j in dataP1.Joints)
                {
                    switch (j.ID)
                    {
                        case JointID.HandLeft:
                            p[0, 0] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.WristLeft:
                            p[0, 1] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.ElbowLeft:
                            p[0, 2] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.HandRight:
                            p[0, 3] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.WristRight:
                            p[0, 4] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.ElbowRight:
                            p[0, 5] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.ShoulderLeft:
                            p[0, 6] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.ShoulderRight:
                            p[0, 7] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.FootLeft:
                            p[0, 8] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.AnkleLeft:
                            p[0, 9] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.KneeLeft:
                            p[0, 10] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.FootRight:
                            p[0, 11] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.AnkleRight:
                            p[0, 12] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.KneeRight:
                            p[0, 13] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.HipLeft:
                            p[0, 14] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.HipRight:
                            p[0, 15] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.Head:
                            p[0, 16] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.Spine:
                            p[0, 17] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                    }
                }

                // Centre the data
                var center = new Point3D((p[0, 6].X + p[0, 7].X) / 2, (p[0, 6].Y + p[0, 7].Y) / 2, (p[0, 6].Z + p[0, 7].Z) / 2);
                // Normalization of the coordinates
                double shoulderDist =
                    Math.Sqrt(Math.Pow((p[0, 6].X - p[0, 7].X), 2) +
                              Math.Pow((p[0, 6].Y - p[0, 7].Y), 2) +
                              Math.Pow((p[0, 6].Z - p[0, 7].Z), 2));

                for (int i = 0; i < 8; i++)
                {
                    p[0, i].X -= center.X;
                    p[0, i].Y -= center.Y;
                    p[0, i].Z -= center.Z;

                    p[0, i].X /= shoulderDist;
                    p[0, i].Y /= shoulderDist;
                    p[0, i].Z /= shoulderDist;
                }
                
                p[0, 16].X -= center.X;
                p[0, 16].Y -= center.Y;
                p[0, 16].Z -= center.Z;

                p[0, 16].X /= shoulderDist;
                p[0, 16].Y /= shoulderDist;
                p[0, 16].Z /= shoulderDist;

                // Centre the data
                center = new Point3D((p[0, 14].X + p[0, 15].X) / 2, (p[0, 14].Y + p[0, 15].Y) / 2, (p[0, 14].Z + p[0, 15].Z) / 2);
                // Normalization of the coordinates
                double hipDist =
                    Math.Sqrt(Math.Pow((p[0, 14].X - p[0, 15].X), 2) +
                              Math.Pow((p[0, 14].Y - p[0, 15].Y), 2) +
                              Math.Pow((p[0, 14].Z - p[0, 15].Z), 2));

                for (int i = 8; i < 16; i++)
                {
                    p[0, i].X -= center.X;
                    p[0, i].Y -= center.Y;
                    p[0, i].Z -= center.Z;

                    p[0, i].X /= hipDist;
                    p[0, i].Y /= hipDist;
                    p[0, i].Z /= hipDist;
                }
            }

            if (dataP2 != null)
            {
                isEmptyP2 = false;

                foreach (Joint j in dataP2.Joints)
                {
                    switch (j.ID)
                    {
                        case JointID.HandLeft:
                            p[1, 0] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.WristLeft:
                            p[1, 1] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.ElbowLeft:
                            p[1, 2] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.HandRight:
                            p[1, 3] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.WristRight:
                            p[1, 4] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.ElbowRight:
                            p[1, 5] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.ShoulderLeft:
                            p[1, 6] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.ShoulderRight:
                            p[1, 7] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.FootLeft:
                            p[1, 8] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.AnkleLeft:
                            p[1, 9] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.KneeLeft:
                            p[1, 10] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.FootRight:
                            p[1, 11] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.AnkleRight:
                            p[1, 12] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.KneeRight:
                            p[1, 13] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.HipLeft:
                            p[1, 14] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.HipRight:
                            p[1, 15] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.Head:
                            p[1, 16] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                        case JointID.Spine:
                            p[1, 17] = new Point3D(j.Position.X, j.Position.Y, j.Position.Z);
                            break;
                    }
                }

                // Centre the data
                var center = new Point3D((p[1, 6].X + p[1, 7].X) / 2, (p[1, 6].Y + p[1, 7].Y) / 2, (p[1, 6].Z + p[1, 7].Z) / 2);
                // Normalization of the coordinates
                double shoulderDist =
                    Math.Sqrt(Math.Pow((p[1, 6].X - p[1, 7].X), 2) +
                              Math.Pow((p[1, 6].Y - p[1, 7].Y), 2) +
                              Math.Pow((p[1, 6].Z - p[1, 7].Z), 2));

                for (int i = 0; i < 8; i++)
                {
                    p[1, i].X -= center.X;
                    p[1, i].Y -= center.Y;
                    p[1, i].Z -= center.Z;

                    p[1, i].X /= shoulderDist;
                    p[1, i].Y /= shoulderDist;
                    p[1, i].Z /= shoulderDist;
                }

                p[1, 16].X -= center.X;
                p[1, 16].Y -= center.Y;
                p[1, 16].Z -= center.Z;

                p[1, 16].X /= shoulderDist;
                p[1, 16].Y /= shoulderDist;
                p[1, 16].Z /= shoulderDist;

                // Centre the data
                center = new Point3D((p[1, 14].X + p[1, 15].X) / 2, (p[1, 14].Y + p[1, 15].Y) / 2, (p[1, 14].Z + p[1, 15].Z) / 2);
                // Normalization of the coordinates
                double hipDist =
                    Math.Sqrt(Math.Pow((p[1, 14].X - p[1, 15].X), 2) +
                              Math.Pow((p[1, 14].Y - p[1, 15].Y), 2) +
                              Math.Pow((p[1, 14].Z - p[1, 15].Z), 2));

                for (int i = 8; i < 16; i++)
                {
                    p[1, i].X -= center.X;
                    p[1, i].Y -= center.Y;
                    p[1, i].Z -= center.Z;

                    p[1, i].X /= hipDist;
                    p[1, i].Y /= hipDist;
                    p[1, i].Z /= hipDist;
                }
            }
            //Spine Coordinates are unnormalized/absolute

            // Launch the event!

            Skeleton3DdataCoordReady(null, new Skeleton3DdataCoordEventArgs(p, isEmptyP1, isEmptyP2));
        }
    }
}